// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MatrixExtensions.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.Diagnostics;
    using ShoNS.Array;

    /// <summary>
    /// Extension methods for matrix
    /// </summary>
    public static class MatrixExtensions
    {
        /// <summary>
        /// Computes the identify matrix of a given dimension
        /// </summary>
        /// <param name="n">The dimension</param>
        /// <returns>The identity matrix</returns>
        public static double[,] GetIdentityMatrix(int n)
        {
            double[,] identMatrix = null;
            if (n < 0)
            {
                throw new ArgumentOutOfRangeException("n");
            }

            identMatrix = new double[n, n];
            for (int i = 0; i < n; i++)
            {
                identMatrix[i, i] = 1;
            }

            return identMatrix;
        }

        /// <summary>
        /// Multiplication of matrix1 by matrix2
        /// </summary>
        /// <param name="matrix1">The first matrix</param>
        /// <param name="matrix2">The second matrix</param>
        /// <returns>The product if dimensions are compatible, null otherwise</returns>
#pragma warning disable 3006
        public static double[,] Multiply(this double[,] matrix1, double[,] matrix2)
        {
            double[,] product = null;
            if (matrix1.GetLength(1) != matrix2.GetLength(0))
            {
                return product;
            }

            product = new double[matrix1.GetLength(0), matrix2.GetLength(1)];
            for (int i = 0; i < matrix1.GetLength(0); i++)
            {
                for (int j = 0; j < matrix2.GetLength(1); j++)
                {
                    for (int k = 0; k < matrix1.GetLength(1); k++)
                    {
                        product[i, j] += matrix1[i, k] * matrix2[k, j];
                    }
                }
            }

            return product;
        }

        /// <summary>
        /// Multiplication of matrix by a vector
        /// </summary>
        /// <param name="matrix">The first matrix</param>
        /// <param name="vector">The second vector</param>
        /// <returns>The product (a vector) if dimensions are compatible for multiplication. null otherwise</returns>
        public static double[] Multiply(this double[,] matrix, double[] vector)
        {
            double[] product = null;
            if (matrix.GetLength(1) != vector.Length)
            {
                return product;
            }

            product = new double[matrix.GetLength(0)];
            for (int i = 0; i < matrix.GetLength(0); i++)
            {
                for (int k = 0; k < matrix.GetLength(1); k++)
                {
                    product[i] += matrix[i, k] * vector[k];
                }
            }

            return product;
        }

#pragma warning restore 3006

        /// <summary>
        /// Transpose of matrix A 
        /// </summary>
        /// <param name="matrix">The matrix</param>
        /// <returns>The transpose</returns>
        public static double[,] Transpose(this double[,] matrix)
        {
            double[,] transpose = new double[matrix.GetLength(1), matrix.GetLength(0)];
            for (int i = 0; i < matrix.GetLength(0); i++)
            {
                for (int j = 0; j < matrix.GetLength(1); j++)
                {
                    transpose[j, i] = matrix[i, j];
                }
            }

            return transpose;
        }

#pragma warning disable 3006

        /// <summary>
        /// Computes difference between two matrix
        /// </summary>
        /// <param name="matrix1">The first matrix</param>
        /// <param name="matrix2">The second matrix</param>
        /// <returns>The difference</returns>
        public static double[,] Subtract(this double[,] matrix1, double[,] matrix2)
        {
            double[,] diff = null;
            if (matrix1.GetLength(0) != matrix2.GetLength(0) || matrix1.GetLength(1) != matrix2.GetLength(1))
            {
                return diff;
            }

            diff = new double[matrix1.GetLength(0), matrix1.GetLength(1)];

            for (int i = 0; i < matrix1.GetLength(0); i++)
            {
                for (int j = 0; j < matrix1.GetLength(1); j++)
                {
                    diff[i, j] = matrix1[i, j] - matrix2[i, j];
                }
            }

            return diff;
        }

        /// <summary>
        /// Computes difference between two vectors 
        /// </summary>
        /// <param name="vector1">The first vector</param>
        /// <param name="vector2">The second vector</param>
        /// <returns>The difference vector</returns>
        public static double[] Subtract(this double[] vector1, double[] vector2)
        {
            double[] diff = null;
            if (vector1.Length != vector2.Length)
            {
                return diff;
            }

            diff = new double[vector1.Length];
            for (int i = 0; i < vector1.Length; i++)
            {
                diff[i] = vector1[i] - vector2[i];
            }

            return diff;
        }

        /// <summary>
        /// Adds 2 matrices
        /// </summary>
        /// <param name="matrix1">The first matrix</param>
        /// <param name="matrix2">The second matrix</param>
        /// <returns>The sum matrix</returns>
        public static double[,] Add(this double[,] matrix1, double[,] matrix2)
        {
            double[,] sum = null;
            if (matrix1.GetLength(0) != matrix2.GetLength(0) || matrix1.GetLength(1) != matrix2.GetLength(1))
            {
                return sum;
            }

            sum = new double[matrix1.GetLength(0), matrix1.GetLength(1)];
            for (int i = 0; i < matrix1.GetLength(0); i++)
            {
                for (int j = 0; j < matrix1.GetLength(1); j++)
                {
                    sum[i, j] = matrix1[i, j] + matrix2[i, j];
                }
            }

            return sum;
        }

        /// <summary>
        /// Computes difference between two vectors and stores the result in the third vector that is passed in
        /// </summary>
        /// <param name="vector1">The first vector</param>
        /// <param name="vector2">The second vector</param>
        /// <param name="diff">The difference vector: diff = vector1 - vector2 on successful completion. Memory has to be allocated apriori for this vector. </param>
        public static void Subtract(this double[] vector1, double[] vector2, double[] diff)
        {
            // Make sure that the lengths match for subtracting the vectors
            Debug.Assert((vector1.Length == vector2.Length), "CopyValues: Input Vectors are not of the same length.");
            Debug.Assert((vector1.Length == diff.Length), "CopyValues: Input and Output vectors are not of the same length.");

            for (int i = 0; i < vector1.Length; i++)
            {
                diff[i] = vector1[i] - vector2[i];
            }
        }

        /// <summary>
        /// Copies values from input array to output array. Assumes that output is initialized to be same shape as input
        /// </summary>
        /// <param name="input">The input vector</param>
        /// <param name="output">The output vector where values from input are copied to. output = input on successful completion. Memory has to be allocated apriori for this vector.</param>
        public static void CopyValues(this double[] input, double[] output)
        {
            // Make sure that the lengths match for copying data
            Debug.Assert((input.Length == output.Length), "CopyValues: Vectors are not of the same length.");

            for (int i = 0; i < input.Length; i++)
            {
                output[i] = input[i];
            }
        }

        /// <summary>
        /// Scale values in input vector by scalar and store it in the same vector (note that the input is modified)
        /// </summary>
        /// <param name="vector">The input vector(Also output). vector = scalar * vector on successful function completion. Memory has to be allocated apriori for this vector</param>
        /// <param name="scalar">Scalar by which to scale the elements of the vector</param>
        public static void Scale(this double[] vector, double scalar)
        {
            Debug.Assert((vector != null), "Scale: Input vector is null.");
            for (int i = 0; i < vector.Length; i++)
            {
                vector[i] *= scalar;
            }
        }

        /// <summary>
        /// Scale values in input matrix by scalar and store it in the same matrix (note that the input is modified)
        /// </summary>
        /// <param name="matrix">The input matrix (Also output). matrix = scalar * matrix on successful function completion. Memory has to be allocated apriori for this matrix </param>
        /// <param name="scalar">Scalar by which to scale the elements of the matrix</param>
        public static void Scale(this double[,] matrix, double scalar)
        {
            Debug.Assert((matrix != null), "Scale: Input matrix is null.");
            for (int i = 0; i < matrix.GetLength(0); i++)
            {
                for (int j = 0; j < matrix.GetLength(1); j++)
                {
                    matrix[i, j] *= scalar;
                }
            }
        }

        /// <summary>
        /// Sets the elements of the input vector to zero (note that the input is modified)
        /// </summary>
        /// <param name="vector">The input vector (Also output). All elements of vector are zero on successful function completion. Memory has to be allocated apriori for this vector. </param>
        public static void SetZero(this double[] vector)
        {
            Debug.Assert((vector != null), "Scale: Input vector is null.");
            for (int i = 0; i < vector.Length; i++)
            {
                vector[i] = 0.0;
            }
        }

        /// <summary>
        /// Sets the elements of the input vector to zero (note that the input is modified)
        /// </summary>
        /// <param name="matrix">The input matrix(Also output). All elements of matrix are zero on successful function completion. Memory has to be allocated apriori for this matrix. </param>
        public static void SetZero(this double[,] matrix)
        {
            Debug.Assert((matrix != null), "Scale: Input matrix is null.");
            for (int i = 0; i < matrix.GetLength(0); i++)
            {
                for (int j = 0; j < matrix.GetLength(1); j++)
                {
                    matrix[i, j] = 0.0;
                }
            }
        }

        /// <summary>
        /// Computes the dot product of the two input vectors
        /// </summary>
        /// <param name="vector1">Input vector 1</param>
        /// <param name="vector2">Input vector 2</param>
        /// <returns>Scalar equal to the dot product of the two vectors or -1 if the vectors are not of the same size </returns>
        public static double VectorInnerProduct(this double[] vector1, double[] vector2)
        {
            // Make sure that the lengths match for inner product
            Debug.Assert((vector1.Length == vector2.Length), "VectorInnerProduct: Input vectors are not of the same length.");

            double scal = 0.0;
            for (int i = 0; i < vector1.Length; i++)
            {
                scal += vector1[i] * vector2[i];
            }

            return scal;
        }

        /// <summary>
        /// Adds 2 vectors 
        /// </summary>
        /// <param name="vector1">The first vector</param>
        /// <param name="vector2">The second vector</param>
        /// <returns>The sum vector</returns>
        public static double[] Add(this double[] vector1, double[] vector2)
        {
            double[] sum = null;
            if (vector1.Length != vector2.Length)
            {
                return sum;
            }

            sum = new double[vector1.Length];
            for (int i = 0; i < vector1.Length; i++)
            {
                sum[i] = vector1[i] + vector2[i];
            }

            return sum;
        }

#pragma warning restore 3006

        /// <summary>
        /// Invert a matrix of arbitrary size input as a 2D array in place
        /// </summary>
        /// <param name="array">The Matrix</param>
        /// <returns>Returns null such as dimension mismatch or singular matrix</returns>
        public static double[,] Invert(this double[,] array)
        {
            double[,] inverse = null;

            int n = array.GetLength(0);
            if (n != array.GetLength(1))
            {
                return inverse;
            }

            int status;
            double[,] a = new double[n, n];
            Array.Copy(array, a, a.Length);

            int i, j;
            int[] indx = new int[n];
            double[,] y = new double[n, n];
            double[] col = new double[n];
            double d = 0.0;

            status = LUDecomp(a, n, indx, ref d);
            if (status != 0)
            {
                for (j = 0; j < n; j++)
                {
                    for (i = 0; i < n; i++)
                    {
                        col[i] = 0.0;
                    }

                    col[j] = 1.0;
                    LUBackSubstitute(a, n, indx, col);
                    for (i = 0; i < n; i++)
                    {
                        y[i, j] = col[i];
                    }
                }

                inverse = new double[n, n];
                for (i = 0; i < n; i++)
                {
                    for (j = 0; j < n; j++)
                    {
                        inverse[i, j] = (double)y[i, j];
                    }
                }
            }

            return inverse;
        }

        /// <summary>
        /// Get the maximum absolute value of all the cells in the matrix
        /// </summary>
        /// <param name="m">A matrix</param>
        /// <returns>The max absolute value</returns>
        public static double MaxAbsoluteValue(DoubleArray m)
        {
            return Math.Max(Math.Abs(m.Min()), m.Max());
        }

        /// <summary>
        /// Helper method for matrix inversion. Decomposes matrix into lower and upper triangular matrix
        /// </summary>
        /// <param name="a">The matrix</param>
        /// <param name="n">The dimension</param>
        /// <param name="indx">The index parameter</param>
        /// <param name="d">The d parameter</param>
        /// <returns>Returns 1 if LU decomp is successful. 0 otherwise (i.e for a singular matrix)</returns>
        private static int LUDecomp(double[,] a, int n, int[] indx, ref double d)
        {
            const double TinyValue = 1.0e-20;
            int i, imax = 0, j, k;
            double big, dum, sum, temp;
            double[] vv;

            vv = new double[n];
            d = 1.0;
            for (i = 0; i < n; i++)
            {
                big = 0.0;
                for (j = 0; j < n; j++)
                {
                    if ((temp = Math.Abs(a[i, j])) > big)
                    {
                        big = temp;
                    }
                }

                if (big == 0.0)
                {
                    return 0; // Singular matrix 
                }

                vv[i] = 1.0 / big;
            }

            for (j = 0; j < n; j++)
            {
                for (i = 0; i < j; i++)
                {
                    sum = a[i, j];
                    for (k = 0; k < i; k++)
                    {
                        sum -= a[i, k] * a[k, j];
                    }

                    a[i, j] = (double)sum;
                }

                big = 0.0;
                for (i = j; i < n; i++)
                {
                    sum = a[i, j];
                    for (k = 0; k < j; k++)
                    {
                        sum -= a[i, k] * a[k, j];
                    }

                    a[i, j] = (double)sum;
                    if ((dum = vv[i] * Math.Abs(sum)) >= big)
                    {
                        big = dum;
                        imax = i;
                    }
                }

                if (j != imax)
                {
                    for (k = 0; k < n; k++)
                    {
                        dum = a[imax, k];
                        a[imax, k] = a[j, k];
                        a[j, k] = (double)dum;
                    }

                    d = -d;
                    vv[imax] = vv[j];
                }

                indx[j] = imax;
                if (a[j, j] == 0.0f)
                {
                    a[j, j] = (double)TinyValue;
                }

                if (j != n)
                {
                    dum = 1.0 / a[j, j];
                    for (i = j + 1; i < n; i++)
                    {
                        a[i, j] *= (double)dum;
                    }
                }
            }

            return 1;
        }

        /// <summary>
        /// LUBack Substitute helper for matrix inversion
        /// </summary>
        /// <param name="a">The a parameter</param>
        /// <param name="n">The dimension</param>
        /// <param name="indx">The index</param>
        /// <param name="b">The b parameter</param>
        private static void LUBackSubstitute(double[,] a, int n, int[] indx, double[] b)
        {
            int i, ii, ip, j;
            double sum;

            ii = -1;
            for (i = 0; i < n; i++)
            {
                ip = indx[i];
                sum = b[ip];
                b[ip] = b[i];
                if (ii >= 0)
                {
                    for (j = ii; j < i; j++)
                    {
                        sum -= a[i, j] * b[j];
                    }
                }
                else if (sum != 0.0)
                {
                    ii = i;
                }

                b[i] = sum;
            }

            for (i = n - 1; i >= 0; i--)
            {
                sum = b[i];
                for (j = i + 1; j < n; j++)
                {
                    sum -= a[i, j] * b[j];
                }

                b[i] = sum / a[i, i];
            }
        }
    }
}
